#include "inc/hw_types.h"		// tBoolean
#include "inc/hw_memmap.h"
#include "driverlib/uart.h"		// input/output over UART
#include "utils/uartstdio.h"	// input/output over UART
#include "driverlib/gpio.h"
#include "driverlib/sysctl.h"	 


#include "RASLib/motor.h"
#include "RASLib/linesensor.h"


#include "RobzDemo.h"

		unsigned char lineSensorByte; //= ReadLineSensor();
		unsigned int left_zero, right_zero;
		//int lineSensorArray[8];





void initFollow(void)
	{
		InitializeLineSensor();
		InitializeMotors(true, false);
		SetDischargeTime(120);
	}

 




void Follow(void)

	{
	while(true) 
		{
			UpdateSensor();

			if( (right_zero == 0) && (left_zero == 0) )
				{
					SetMotorPowers(50,50);
				}
			
			// This part is tricky.... we are looking for 4 bits set and then we know an extreme curve is up a head.
			// so we will stop one wheel, this is to now over shoot our line as we take the turn.
			else if((left_zero + right_zero) >= 3)
				{

				if(left_zero > right_zero)
					{
						SetMotorPowers(-50, 80);
					}

				if(right_zero > left_zero)
					{
						SetMotorPowers(80, -50);
					}
				//if(right_zero == left_zero)
				//	{
				//		SetMotorPowers(50, 10);
				//	}	

				}
			else if(left_zero > right_zero)
				{
					SetMotorPowers(-30, 80);
				}

			else if(right_zero > left_zero)
				{
					SetMotorPowers(80, -30);
				}
			else   //if(left_zero == right_zero)
				{
	   				SetMotorPowers(50, 50);
				}
			


			
	 		//black is 1, white is 0
	}


 }


   
void UpdateSensor(void)
{
			unsigned int i;
		
			left_zero = 0;
			right_zero = 0;
			lineSensorByte = ReadLineSensor();	 // num bettween 0 255 
			//UARTprintf("%d ", lineSensorByte);	 //0-255


			i = 0;
			for(i = 0; i < 4; i++) {
				if((lineSensorByte & 0x01)==0)
					{left_zero++;}
				lineSensorByte = lineSensorByte >> 1;
			}	

			for(i = 4; i < 8; i++) {
				if((lineSensorByte & 0x01)==0)
					{right_zero++;}
				lineSensorByte = lineSensorByte >> 1;
			}
		

	
}
